﻿using System;
using System.Windows.Forms;
using System.Threading;
using System.Collections.Generic;

namespace TMCM
{
    public class TMCM1110 : TMCM
    {
        private Dictionary<SAP_TYPE, int> settings;

        public TMCM1110(string port_name = "COM4") : base()
        {     
            InitializeValues();

            base.ComSettings = new SerialSettings();
            base.ComSettings.PortName = port_name;
        }

        // instructions
        public enum INST : byte 
        {
            ROR = 0x01,     // 1 rotate to right
            ROL = 0x02,     // 2 rotate to left
            MST = 0x03,     // 3 motor stop
            MVP = 0x04,     // 4 move to position
            SAP = 0x05,     // 5 set axis parameter
            GAP = 0x06,     // 6 get axis parameter

            STAP = 0x07,    // 7 Store axis parameter into EEPROM
            RSAP = 0x08,    // 8 Restore axis parameter from EEPROM
            SGP = 0x09,     // 9 Set global parameter
            GGP = 0x0A,     // 10 Get global parameter
            STGP = 0x0B,    // 11 Store global parameter into EEPROM
            RSGP = 0x0C,    // 12 Restore global parameter from EEPROM
            RFS = 0x0D,     // 13 Reference search
            SIO = 0x0E,     // 14 Set output
            GIO = 0x0F,     // 15 Get input

            SCO = 0x1E,     // 30 Store coordinate
            GCO = 0x1F,     // 31 Get coordinate
            CCO = 0x20,     // 32 Capture coordinate

            CQ = 0x88       // status, who are you?
        }

        // set axis parameter types
        public enum SAP_TYPE : byte
        {
            ActualPosition = 0x01,
            MaxPositioningSpeed = 0x04,       // relative position
            MaxAcceleration = 0x05,           // actual position
            AbsoluteMaxCurrent = 0x06,        //
            StandbyCurrent = 0x07,
            RightLimitSwitchDisable = 0x0C,
            LeftLimitSwitchDisable = 0x0D,
            MinimumSpeed = 0x82,
            RampMode = 0x8A,                  //
            MicrostepResolution = 0x8C,       //
            RefSwitchTolerance = 0x8D,        //
            SoftStopFlag = 0x95,              //
            RampDivisor = 0x99,               //
            PulseDivisor = 0x9A,              //
            ReferencingMode = 0xC1,           //
            ReferencingSearchSpeed = 0xC2,    //
            ReferencingSwitchSpeed = 0xC3,    //
            MixedDecayThreshold = 0xCB,       //
            Freewheeling = 0xCC,              //
            StallDetectionThreshold = 0xCD,   //
            FullstepThreshold = 0xD3,         //
            PowerDownDelay = 0xD6            //
        }

        public enum GAP_TYPE : byte
        {
            TargetPosition = 0x00,
            ActualPosition = 0x01,
            TargetSpeed = 0x02,
            ActualSpeed = 0x03,
            MaxPositioningSpeed = 0x04,       // relative position
            MaxAcceleration = 0x05,           // actual position
            AbsoluteMaxCurrent = 0x06,        //
            StandbyCurrent = 0x07,
            PositionReached = 0x08,
            RefSwitchStatus = 0x09,
            RightLimitSwitchStatus = 0x0A,
            LeftLimitSwitchStatus = 0x0B,
            RightLimitSwitchDisable = 0x0C,
            LeftLimitSwitchDisable = 0x0D,
            MinimumSpeed = 0x82,
            ActualAcceleration = 0x87,
            RampMode = 0x8A,                  //
            MicrostepResolution = 0x8C,       //
            RefSwitchTolerance = 0x8D,        //
            SoftStopFlag = 0x95,              //
            RampDivisor = 0x99,               //
            PulseDivisor = 0x9A,              //
            ReferencingMode = 0xC1,           //
            ReferencingSearchSpeed = 0xC2,    //
            ReferencingSwitchSpeed = 0xC3,    //
            MixedDecayThreshold = 0xCB,       //
            Freewheeling = 0xCC,              //
            StallDetectionThreshold = 0xCD,   //
            FullstepThreshold = 0xD3,         //
            PowerDownDelay = 0xD6             //
        }

        public enum GIO_TYPE : byte
        {
            ADIN0 = 0x00,
            ADIN1 = 0x01,
            ADIN2 = 0x02,
            ADIN3 = 0x03,
            ADIN4 = 0x04,
            ADIN5 = 0x05,
            ADIN6 = 0x06,
            ADIN7 = 0x07
        }

        public enum SIO_TYPE : byte
        {
            DOUT0 = 0x00,
            DOUT1 = 0x01,
            DOUT2 = 0x02,
            DOUT3 = 0x03,
            DOUT4 = 0x04,
            DOUT5 = 0x05,
            DOUT6 = 0x06,
            DOUT7 = 0x07
        }


        private void InitializeValues()
        {
            settings = new Dictionary<SAP_TYPE, int>();

            settings.Add(SAP_TYPE.MicrostepResolution, 6);
            settings.Add(SAP_TYPE.AbsoluteMaxCurrent, 128);
            settings.Add(SAP_TYPE.StandbyCurrent, 5);
            settings.Add(SAP_TYPE.RightLimitSwitchDisable, 0);
            settings.Add(SAP_TYPE.LeftLimitSwitchDisable, 0);
            settings.Add(SAP_TYPE.SoftStopFlag, 0);
            settings.Add(SAP_TYPE.ReferencingMode, 1);
            settings.Add(SAP_TYPE.ReferencingSearchSpeed, 800);
            settings.Add(SAP_TYPE.ReferencingSwitchSpeed, 150);
            settings.Add(SAP_TYPE.MaxAcceleration, 1000);
            settings.Add(SAP_TYPE.MaxPositioningSpeed, 300);
        }


        public new bool Open()
        {
            base.Open();
            foreach (var setting in settings)
            {
                this.SetAxisParameter(setting.Key, setting.Value);
            }
            return true;
        }

        public void RotateLeft(int Speed)
        {
            sendCommand(0x01, (byte)INST.ROL, 0x00, 0x00, Speed);
        }

        public void RotateRight(int Speed)
        {
            sendCommand(0x01, (byte)INST.ROR, 0x00, 0x00, Speed);
        }

        public void MotorStop()
        {
            WaitForComport();
            sendCommand(0x01, (byte)INST.MST, 0x00, 0x00, 0x00);
            Thread.Sleep(100);
        }

        public bool MoveToPosition(MVP_TYPE type, int pos, WAIT_TYPE WaitType, int WaitValue)
        {
            WaitForComport();
            ReceiveData replay = sendCommand(0x01, (byte)INST.MVP, (byte)type, 0x00, pos);
            Thread.Sleep(100);

            switch (WaitType)
            {
                case WAIT_TYPE.Ticks:
                    Thread.Sleep(WaitValue);
                    break;
                case WAIT_TYPE.POS:
                    long Timeout = DateTime.Now.Ticks;
                    bool PositionEnd = false;
                    while (!PositionEnd) // && (DateTime.Now.Ticks - Timeout) < ((WaitValue == 0 ? 30 : WaitValue) * 10000000)
                    {
                        PositionEnd = Convert.ToBoolean(GetAxisParameter(GAP_TYPE.PositionReached));
                    }
                    break;
                case WAIT_TYPE.LIMSW:
                    bool triggered = false;
                    int counter = 0;
                    while (counter < 10)
                    {
                        triggered = Convert.ToBoolean(GetAxisParameter(GAP_TYPE.LeftLimitSwitchStatus)) || Convert.ToBoolean(GetAxisParameter(GAP_TYPE.RightLimitSwitchStatus));
                        if (triggered)
                        {
                            counter++;
                        }
                        else { counter = 0; }
                    }
                    break;
            }
            return (replay.Status == 100);
        }

        public bool ReferencSearch()
        {
            WaitForComport();
            ReceiveData replay = sendCommand(0x01, (byte)INST.RFS, 0x00, 0x00, 0);
            Thread.Sleep(100);

            long Timeout = DateTime.Now.Ticks;
            bool PositionEnd = false;
            while (!PositionEnd && (DateTime.Now.Ticks - Timeout) < (120 * 10000000))
            {
                PositionEnd = (sendCommand(0x01, (byte)INST.RFS, 0x02, 0x00, 0).Value == 0);
            }

            return PositionEnd;
        }

        public bool SetAxisParameter(SAP_TYPE type, int Value)
        {
            WaitForComport();
            ReceiveData replay = sendCommand(0x01, (byte)INST.SAP, (byte)type, 0x00, Value);
            return (replay.Status == 100);
        }

        public int GetAxisParameter(GAP_TYPE type)
        {
            WaitForComport();
            ReceiveData replay = sendCommand(0x01, (byte)INST.GAP, (byte)type, 0x00, 0);
            return replay.Value;
        }


        public bool SetOutput(SIO_TYPE type, byte bank, int Value)
        {
            WaitForComport();
            ReceiveData replay = sendCommand(0x01, (byte)INST.SIO, (byte)type, bank, Value);
            return (replay.Status == 100);
        }

        public int GetInput(GIO_TYPE type, byte bank)
        {
            WaitForComport();
            ReceiveData replay = sendCommand(0x01, (byte)INST.GIO, (byte)type, bank, 0);
            return replay.Value;
        }
       
    }
}